﻿// --------------------------------------------------------------------------------------------------------------------
// <copyright file="FingerPaint.cs" company="RIT Center for Student Innovation">
//      Copyright (C) 2011 ParadigmZero LLC
//
//      This program is free software: you can redistribute it and/or modify
//      it under the terms of the GNU General Public License as published by
//      the Free Software Foundation, either version 3 of the License, or
//      (at your option) any later version.
//
//      This program is distributed in the hope that it will be useful,
//      but WITHOUT ANY WARRANTY; without even the implied warranty of
//      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//      GNU General Public License for more details.
//
//      You should have received a copy of the GNU General Public License
//      along with this program.  If not, see 'http://www.gnu.org/licenses/'.
// </copyright>
// <summary>
//       Fingerpaint Sample Application using the PrimeSense and OpenAI Kinect drivers.
// </summary>
// --------------------------------------------------------------------------------------------------------------------

// This applications assumes you have a working Kinect already set up with the OpenNI drivers on your machine.
// If you need assistance setting up your kinect device there are plenty of sources on the web.  
// One pretty well written article, for windows platforms can be found at the following URL:
//      http://http://www.codeproject.com/Articles/148251/How-to-Successfully-Install-Kinect-on-Windows-Open.aspx
// If you have difficulty getting the samples to work, try uninstalling all the applications and drivers
// and use the instructions in the troubleshooting section.  On my windows 7 setup I had to use the latest
// unstable drivers and the troubleshooting instructions to make a successful connection.  
// Be patient, try different combinations of drivers if you are having difficulty.
// Its also interesting to know that there is no real Kinect specifc code here.  This code should
// in theory work with any OpenNI compatible device that supports the same production nodes.

namespace FingerPaint
{
    using System;
    using System.ComponentModel;
    using System.Drawing;
    using System.Drawing.Imaging;
    using System.Threading;
    using System.Windows.Forms;
    using xn;   // The OpenNI Libary
    using xnv;

    /// <summary>
    /// The Main Window Class for the fingerpaint application.
    /// </summary>
    public partial class FingerPaint : Form
    {
        /// <summary>
        /// The Depth Generator Production Node.
        /// </summary>
        private readonly DepthGenerator depthGenerator;

        /// <summary>
        /// Thread obejct to perform device reads.
        /// </summary>
        private readonly Thread readerThread;

        /// <summary>
        /// The Image Generator Production Node.
        /// </summary>
        private readonly ImageGenerator imageGenerator;

        /// <summary>
        /// OpenNI hands generator production node.
        /// </summary>
        private readonly HandsGenerator handGenerator;

        /// <summary>
        /// OpenNI gesture generator to satisfy the NITE library.
        /// </summary>
        private readonly GestureGenerator gestureGenerator;

        /// <summary>
        /// Bitmap for depth buffer.
        /// </summary>
        private readonly Bitmap frontDepthBuffer;

        /// <summary>
        /// Array to create a histogram of depth data.
        /// </summary>
        private readonly int[] histogram;

        /// <summary>
        /// The OpenAI Context Object.
        /// </summary>
        private readonly Context context;

        /// <summary>
        /// This is the NITE Session manager for gesture tracking.
        /// </summary>
        private readonly SessionManager sessionManager;

        /// <summary>
        /// The NITE flow router for routed gesture event handling.
        /// </summary>
        private readonly FlowRouter flowRouter;

        /// <summary>
        /// The NITE point control is a listener for point(hand) gesture locations.
        /// </summary>
        private readonly PointControl points;

        /// <summary>
        /// Main Loop Flag, set it to fales to exit the program.
        /// </summary>
        private bool running;

        /// <summary>
        /// Initializes a new instance of the <see cref="FingerPaint"/> class.
        /// </summary>
        public FingerPaint()
        {
            InitializeComponent();

            // Initialize OpenNI... 
            // Create a context object.  The context object holds the application state of OpenNI base applications
            // The context takes the path to a device configuration file.  See the OpenAI documentation for 
            // instructions on the implementation of this file.
            this.context = new Context("KinectConfig.xml");

            // Get the necessary production nodes for interacting with the Kinect Data directly
            try
            {
                // Get the production node for the depth generator.  This node will be used for determining position 
                // in relation to the kinect.
                this.depthGenerator = this.context.FindExistingNode(NodeType.Depth) as DepthGenerator;
                if (this.depthGenerator == null)
                {
                    throw new Exception(@"Could not find depth node");
                }

                // The image generator will be used to capture some video and images, primarily for goofing around and doing other 
                // fun image manipulations.
                this.imageGenerator = this.context.FindExistingNode(NodeType.Image) as ImageGenerator;
                if (this.imageGenerator == null)
                {
                    throw new Exception(@"Could not find image node");
                }

                // Initialize the hands generator.. This allows for hand tracking support
                //this.handGenerator = this.context.FindExistingNode(NodeType.Hands) as HandsGenerator;
                //if (this.handGenerator == null)
                //{
                //    throw new Exception(@"Could not find the hands node");
                //}


                this.handGenerator = new HandsGenerator(this.context);

                // Initialize a gesture geneator (needed to establish the NITE session at least in the .net library)
                this.gestureGenerator = new GestureGenerator(this.context);
             

            }
            catch (XnStatusException ex)
            {
                MessageBox.Show(@"Could not create a Node:\n" + ex.Message, @"Exception: " + ex.Message, MessageBoxButtons.OK, MessageBoxIcon.Error);
                throw;
            }

        
            // Initialize the NITE objects...
            // Start the nite session manager
            try
            {
                this.sessionManager = new SessionManager(this.context, "Click,Wave", "RaiseHand");
                this.sessionManager.SessionEnd += this.sessionManager_SessionEnd;
                this.sessionManager.SessionFocusProgress += this.sessionManager_SessionFocusProgress;
                this.sessionManager.SessionStart += this.sessionManager_SessionStart;
            }
            catch (Exception ex)
            {
                MessageBox.Show(@"Could not start NITE session:\n" + ex.Message, @"Exception: " + ex.Message, MessageBoxButtons.OK, MessageBoxIcon.Error);
                throw;
            }

            // set up the flow router to listen for points (hands)
            this.points = new PointControl();
            

            this.flowRouter = new FlowRouter();
            this.flowRouter.SetActive(this.points);

            // add it to the session manager
            this.sessionManager.AddListener(this.flowRouter);

            // point control init...
            this.points.NoPoints += this.points_NoPoints;
            this.points.PrimaryPointUpdate += this.points_PrimaryPointUpdate;
            this.points.PointUpdate += this.points_PointUpdate;
            this.points.OnUpdate += this.points_OnUpdate;

            this.running = true;

            // tell OpenNI to start generating data
            this.context.StartGeneratingAll();

            // map mode containd the resolution and FPS of the device..
            // for the kinect it shouls alawys be 640x480 30fps.  
            // Milage may vary with other OpenNI compatible devices.
            MapOutputMode mapMode = this.imageGenerator.GetMapOutputMode();
            this.frontDepthBuffer = new Bitmap((int)mapMode.nXRes, (int)mapMode.nYRes, System.Drawing.Imaging.PixelFormat.Format24bppRgb);

            // Initialize the histogram, allocate its memory array
            this.histogram = new int[this.depthGenerator.GetDeviceMaxDepth()];

            // Start the thread responsible for reading from the Kinect
            this.readerThread = new Thread(this.ReaderThread);
            this.readerThread.Start();
        }

        public unsafe void points_OnUpdate(IntPtr message)
        {
            //HandPointContext context = *(HandPointContext*)message;
            //System.Diagnostics.Debug.WriteLine("Tracking: ({0:F},{1:F},{2:f}) User: {3:d} Confidence: {4:f} ", context.ptPosition.X, context.ptPosition.Y, context.ptPosition.Z, context.nUserID, context.fConfidence);
            points.UpdateMessage(message);
        }

        void points_PointUpdate(ref HandPointContext context)
        {
            System.Diagnostics.Debug.WriteLine("Tracking: ({0:F},{1:F},{2:f}) User: {3:d} Confidence: {4:f} ", context.ptPosition.X, context.ptPosition.Y, context.ptPosition.Z, context.nUserID, context.fConfidence);
        }

        void points_PrimaryPointUpdate(ref HandPointContext context)
        {
            System.Diagnostics.Debug.WriteLine("Tracking: ({0:F},{1:F},{2:f}) User: {3:d} Confidence: {4:f} ", context.ptPosition.X, context.ptPosition.Y, context.ptPosition.Z, context.nUserID, context.fConfidence);
        }

        void points_NoPoints()
        {
            throw new NotImplementedException();
        }

        void sessionManager_SessionStart(ref Point3D ptPosition)
        {
            System.Diagnostics.Debug.WriteLine("Session start: ({0:F},{1:F},{2:f})\n", ptPosition.X, ptPosition.Y, ptPosition.Z);
            //g_SessionState = IN_SESSION;
        }

        void sessionManager_SessionFocusProgress(string strFocus, ref Point3D ptPosition, float fProgress)
        {
            System.Diagnostics.Debug.WriteLine("Focus progress: {0} @({1:F},{2:F},{3:f}): {4:f}", strFocus, ptPosition.X, ptPosition.Y, ptPosition.Z, fProgress);
        }

        void sessionManager_SessionEnd()
        {
            throw new NotImplementedException();
        }

        /// <summary>
        /// Main function for the Reader Thread.
        /// </summary>
        protected unsafe void ReaderThread()
        {
            DepthMetaData depthMd = new DepthMetaData();
            ImageMetaData imageMd = new ImageMetaData();

            while (this.running)
            {
                try
                {
                    this.context.WaitOneUpdateAll(this.handGenerator);
                }
                catch (Exception ex)
                {
                    System.Diagnostics.Debug.WriteLine(@"WaitOneUpdateAll() Exception:" + ex.Message);
                }

                this.depthGenerator.GetMetaData(depthMd);
                this.imageGenerator.GetMetaData(imageMd);

                // Calculate the historgram for the depth data
                this.CalcHist(depthMd);

                lock (this)
                {
                    Rectangle rect = new Rectangle(0, 0, this.frontDepthBuffer.Width, this.frontDepthBuffer.Height);
                    BitmapData data = this.frontDepthBuffer.LockBits(rect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);

                    byte* ptrImage = (byte*)imageMd.ImageMapPtr;

                    // set pixels for image...
                    ushort* ptrDepth = (ushort*)this.depthGenerator.GetDepthMapPtr().ToPointer();
                    for (int y = 0; y < imageMd.YRes; ++y)
                    {
                        byte* ptrDest = (byte*)data.Scan0.ToPointer() + (y * data.Stride);
                        for (int x = 0; x < imageMd.XRes; ++x, ptrImage += 3, ptrDest += 3, ++ptrDepth)
                        {
                            // This snippet displays the depth histogram in a yellow tint
                            // byte pixel = (byte)this.histogram[*pDepth];
                            //        pDest[0] = 0;
                            //        pDest[1] = pixel;
                            //        pDest[2] = pixel;

                             // This block displays the image data
                            ptrDest[0] = ptrImage[2];
                            ptrDest[1] = ptrImage[1];
                            ptrDest[2] = ptrImage[0];

                            // The rest of these are some sparkadelic combinations of the depth buffer data
                            // pDest[0] = (byte)((((byte)(pImage[2])) + (*pDepth)) % 256);//md[x,y].nBlue;
                            // pDest[1] = (byte)((((byte)(pImage[1])) + (*pDepth)) % 256); //md[x,y].nBlue;
                            // pDest[2] = (byte)((((byte)(pImage[0])) + (*pDepth)) % 256);//md[x,y].nBlue;

                            // pDest[0] = (byte)((0 + (*pDepth)) % 768);//md[x,y].nBlue;
                            // pDest[1] = (byte)((0 + (*pDepth)) % 768);//md[x,y].nBlue;
                            // pDest[2] = (byte)((0 + (*pDepth)) % 768);//md[x,y].nBlue;

                            // pDest[0] = (byte)Math.Min((((0 + (*pDepth)) % 256) - 000),0);//md[x,y].nBlue;
                            // pDest[1] = (byte)Math.Min((((0 + (*pDepth)) % 512) - 255),0);//md[x,y].nBlue;
                            // pDest[2] = (byte)Math.Min((((0 + (*pDepth)) % 768) - 511),0);//md[x,y].nBlue;
                            
                            // int val = (int)((*ptrDepth) * 1677.7216);
                            // ptrDest[0] = (byte)((val & 0xFF0000) >> 16); // md[x,y].nBlue;
                            // ptrDest[1] = (byte)((val & 0x00FF00) >> 8); // md[x,y].nBlue;
                            // ptrDest[2] = (byte)(val & 0x0000FF); // md[x,y].nBlue;
                        }
                    }
                    
                    this.frontDepthBuffer.UnlockBits(data);
                }

                this.Invalidate();
            }
        }

        /// <summary>
        /// Calculate a histogram of the depth data.  Results are stored in an internal private array.
        /// </summary>
        /// <param name="depthMd">
        /// The DepthMetaData object, used to access the depth data.
        /// </param>
        protected unsafe void CalcHist(DepthMetaData depthMd)
        {
            // reset
            for (int i = 0; i < this.histogram.Length; ++i)
            {
                this.histogram[i] = 0;
            }

            ushort* ptrDepth = (ushort*)depthMd.DepthMapPtr.ToPointer();

            int points = 0;
            for (int y = 0; y < depthMd.YRes; ++y)
            {
                for (int x = 0; x < depthMd.XRes; ++x, ++ptrDepth)
                {
                    ushort depthVal = *ptrDepth;
                    if (depthVal != 0)
                    {
                        this.histogram[depthVal]++;
                        points++;
                    }
                }
            }

            for (int i = 1; i < this.histogram.Length; i++)
            {
                this.histogram[i] += this.histogram[i - 1];
            }

            if (points <= 0)
            {
                return;
            }

            for (int i = 1; i < this.histogram.Length; i++)
            {
                this.histogram[i] = (int)(256 * (1.0f - (this.histogram[i] / (float)points)));
            }
        }

        /// <summary>
        /// Override the on paint function to draw on our display panel.
        /// </summary>
        /// <param name="e">
        /// The PaintEventArgs object.
        /// </param>
        protected override void OnPaint(PaintEventArgs e)
        {
            base.OnPaint(e);

            lock (this)
            {
                // TODO: This draw routine isnt the fastest in the world, look at a replacement.
                e.Graphics.DrawImage(
                    this.frontDepthBuffer,
                    this.depthView.Location.X,
                    this.depthView.Location.Y,
                    this.depthView.Size.Width,
                    this.depthView.Size.Height);
            }
        }

        /// <summary>
        /// Override the background paint routine to prevent it from interfering with our panel repaint.
        /// </summary>
        /// <param name="paintEventArgs">
        /// The pevent.
        /// </param>
        protected override void OnPaintBackground(PaintEventArgs paintEventArgs)
        {
            // Don't allow the background to paint
        }

        /// <summary>
        /// Override the closing event so we can clean up our resources.
        /// </summary>
        /// <param name="e">
        /// Closing event arguments.
        /// </param>
        protected override void OnClosing(CancelEventArgs e)
        {
            this.running = false;
            this.readerThread.Join();
            base.OnClosing(e);
        }

        /// <summary>
        /// Override the key press so we can act on keyboard inputs.
        /// </summary>
        /// <param name="e">
        /// Key press even arguments.
        /// </param>
        protected override void OnKeyPress(KeyPressEventArgs e)
        {
            if (e.KeyChar == 27)
            {
                Close();
            }

            base.OnKeyPress(e);
        }
    }
}
